//==============================================================================
//
//  File        : sensor_OV5650.c
//  Description : Firmware Sensor Control File
//  Author      : Philip Lin
//  Revision    : 1.0
//
//=============================================================================

#include "includes_fw.h"
#if defined(DSC_MP3_P_FW)||defined(VIDEO_AMR_R_FW)||defined(VIDEO_AAC_R_FW)||(SENSOR_EN)
#include "config_fw.h"

#if BIND_SENSOR_OV5650

#include "lib_retina.h"
#include "reg_retina.h"

#include "mmp_reg_vif.h"
#include "mmp_reg_scaler.h"
#include "mmp_reg_gpio.h"
#include "mmp_reg_rawproc.h"

#include "mmpf_sensor.h"
#include "mmpf_hif.h"
#include "mmpf_i2cm.h"
#include "mmpf_vif.h"
#include "isp_if.h"
#include "mmpf_usbpccam.h"

#include "mmp_reg_i2cm.h"
#include "mmps_3gprecd.h"
#include "sensor_OV5650.h"
#include "hdm_ctl.h"
#include "mmpf_dram.h"
#include "motor_ctl.h"
#include "flash_ctl.h"
#include "mmpf_pll.h"

MMPF_I2CM_ATTRIBUTE m_snr_I2cmAttribute = {
            SENSOR_I2CM_ID, SENSOR_I2C_ADDR_OV5650, 16, 8, 0, MMP_FALSE, MMP_FALSE, MMP_FALSE,
            MMP_FALSE, 0, 0, 0, MMPF_I2CM_SPEED_HW_250K, NULL, NULL};

#ifndef SENSOR_ID_OV5650
    #define SENSOR_ID_OV5650 0
#endif
static	MMP_UBYTE   vif_id = 0;

#define G2_CLK      (120)

extern MMP_USHORT  gsSensorPreviewWidth[], gsSensorPreviewHeight[];
extern MMP_USHORT  gsCurPreviewMode, gsCurPhaseCount;
//extern MMP_USHORT  gsSensorMode;
extern MMP_USHORT  m_usVideoQuality;
extern MMP_USHORT  m_gsISPCoreID;

extern MMPS_3GPRECD_VIDEO_FORMAT m_VideoFmt;
extern MMP_BOOL gbAutoFocus;

extern MMP_USHORT m_usResolType;
extern MMP_ULONG  glPccamResolution;

const __align(4) ISP_UINT8 Sensor_IQ_CompressedText[] = {
#include "isp_8451_iq_data_v2_OV5650.xls.ciq.txt"
};

#define  SENSOR_ROTATE_180  0   
#define SUPPORT_ONE_LANE_PREVIEW    0

#define  PIXEL_RATE_60M     (1)
#define  PIXEL_RATE_42M     (2)

#define VOLTAGE33V 0
#define VOLTAGE28V 1
#define VOLTAGE18V 2

#define SENSOR_VOLTAGE      VOLTAGE18V
//#if (G1_CLK < 120000)
    #define  SENSOR_PIXEL_RATE  (PIXEL_RATE_42M)
//#else
//    #define  SENSOR_PIXEL_RATE    (PIXEL_RATE_60M)
//#endif

//#define SUPPORT_AUTO_FOCUS 0

//#include "sensor_ctl.h"
//#include "preview_ctl.h"
//#include "XDATAmap.h"
//#include "3A_ctl.h"
//#include "OV5650.h"
//#include "motor_ctl.h"

#define BASE_SHUTTER_50HZ_1280x960		(298)
#define BASE_SHUTTER_60HZ_1280x960		(248)
#define BASE_SHUTTER_50HZ_1920x1080		(334)
#define BASE_SHUTTER_60HZ_1920x1080		(278)
#define BASE_SHUTTER_50HZ_2560x1920		(296)
#define BASE_SHUTTER_60HZ_2560x1920		(247)
void SNR_OV5650_SetSensorResolution_Customer(ISP_SENSOR_RESOL res);
void SNR_OV5650_InitSensor_Customer(void);
#if SUPPORT_AUTO_FOCUS
void MOTOR_OV5650_VCM_InitMotor_Customer(void);
void MOTOR_OV5650_VCM_SetAFPosition_Customer(MMP_USHORT af_pos);
#endif

#if(BIND_SENSOR_OV5650)&&(AUDEXT_DAC==WM8960)
MMP_UBYTE SEN_AUD_EN = 0;
#endif
static VENUS_SYSTEM_MODE gSystemMode;

//------------------------------------------------------------------------------
//  Function    : OV5650_Sensor_PreviewMode
//  Description :
//------------------------------------------------------------------------------
extern MMP_BYTE gbSignalType;
extern MMPF_SENSOR_FUNCTION *gsSensorFunction ;
static void OV5650_Sensor_PreviewMode(MMP_USHORT usPreviewmode)
{
    gsCurPreviewMode = usPreviewmode;
    //gsCurPhaseCount = usPhasecount;

    switch (usPreviewmode) {
    case 0:
        RTNA_DBG_Str(3, "Sensor 5M (2560x1920) preview mode\r\n");
        //ISP_IF_3A_Control(ISP_3A_DISABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_FALSE);

        gSystemMode = VENUS_SYSTEM_MODE_SNAPSHOT;//ISP_IF_SNR_SetBeforeCapture();
        ISP_IF_IQ_SetSysMode(0); //Steven NOTE:please reference excel define: 0=capture, 1=preview, 2=video....
        ISP_IF_AE_SetSysMode(0); // switch AE exposure table @20120822 0=camera(5M) , 1=video
        SNR_OV5650_SetSensorResolution_Customer(ISP_SENSOR_RESOL_2560x1920);

        //set color transform 16 ~ 235 for YUV H264
        //MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_0, MMP_TRUE, MMPF_SCALER_COLRMTX_BT601);
        // 0~255 for MJEG
        MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_0, MMP_TRUE, MMPF_SCALER_COLRMTX_FULLRANGE);

        ISP_IF_AE_SetFPS(0);
        //ISP_IF_3A_Control(ISP_3A_ENABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_TRUE);
        break;
    case 1:
        RTNA_DBG_Str(0, "Sensor preview (1280x960) mode\r\n");
        RTNA_DBG_Str(0, "SENSOR_VIDEO_MODE\r\n");
        //ISP_IF_3A_Control(ISP_3A_DISABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_FALSE);

        gSystemMode = VENUS_SYSTEM_MODE_PREVIEW;//ISP_IF_SNR_SetBeforePreview();
        ISP_IF_IQ_SetSysMode(2); //Steven NOTE:please reference excel define: 0=capture, 1=preview, 2=video....
        ISP_IF_AE_SetSysMode(1); // switch AE exposure table @20120822 0=camera(5M) , 1=video
        SNR_OV5650_SetSensorResolution_Customer(ISP_SENSOR_RESOL_1280x960);

        //set color transform 16 ~ 235
        //MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_0, MMP_TRUE, MMPF_SCALER_COLRMTX_BT601);
        // 0~255 for MJEG
        MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_0, MMP_TRUE, MMPF_SCALER_COLRMTX_FULLRANGE);
        ISP_IF_AE_SetFPS(0);
        //ISP_IF_3A_Control(ISP_3A_ENABLE);
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_TRUE);
        break;
    case 2:
        RTNA_DBG_Str(0, "Sensor preview (1920x1080) mode\r\n");
        RTNA_DBG_Str(0, "SENSOR_VIDEO_MODE\r\n");
        //ISP_IF_3A_Control(ISP_3A_DISABLE); //patrick@100518
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_FALSE);

        gSystemMode = VENUS_SYSTEM_MODE_PREVIEW;//ISP_IF_SNR_SetBeforePreview();
        ISP_IF_IQ_SetSysMode(2); //Steven NOTE:please reference excel define: 0=capture, 1=preview, 2=video....
        ISP_IF_AE_SetSysMode(1); // switch AE exposure table @20120822 0=camera(5M) , 1=video

        SNR_OV5650_SetSensorResolution_Customer(ISP_SENSOR_RESOL_1920x1080);

        #if 0	
        MMPF_VIF_GetVIFGrabResolution(&ulWidth, &ulHeight);
        dbg_printf(3, "[2] ulWidth = %d, ulHeight = %d\r\n", ulWidth, ulHeight); 
        #endif

        //set color transform 16 ~ 235
        //MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_0, MMP_TRUE, MMPF_SCALER_COLRMTX_BT601);
        // 0~255 for MJEG
        MMPF_Scaler_SetOutputColor(MMPF_SCALER_PATH_0, MMP_TRUE, MMPF_SCALER_COLRMTX_FULLRANGE);
        ISP_IF_AE_SetFPS(0);
        //ISP_IF_3A_Control(ISP_3A_ENABLE);
        gsSensorFunction->MMPF_Sensor_Set3AStatus(MMP_TRUE);
        break;
    }
}
extern void GPIO_OutputControl(MMP_UBYTE num, MMP_UBYTE status);
extern volatile MMP_UBYTE   gbDramID;
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_Initialize
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_Initialize_OV5650(void)
{
    //AITPS_PAD   pPAD  = AITC_BASE_PAD;
    AITPS_VIF   pVIF = AITC_BASE_VIF;

    RES_TYPE_CFG *cur_res;

    RTNA_DBG_Str(0, "MMPF_Sensor_Initialize_OV5650...Begin\r\n");

    //Init globol variable used by DSC
    gsSensorPreviewWidth[0]  = 2592;//2560;
    gsSensorPreviewHeight[0] = 1944;//1920;
    // use 1.3M for preview, check sensor preview setting
    gsSensorPreviewWidth[1]  = 1280;
    gsSensorPreviewHeight[1] = 960;

    gsSensorPreviewWidth[2]  = 1920;
    gsSensorPreviewHeight[2] = 1080;

    #if 0
    pPAD->PAD_PSCK = PAD_E8_CURRENT | PAD_E4_CURRENT | PAD_PULL_HIGH;
    pPAD->PAD_PSDA = PAD_E4_CURRENT | PAD_PULL_HIGH;
    pPAD->PAD_PSEN = PAD_E4_CURRENT | PAD_PULL_HIGH;
    #endif

    #if (CUSTOMER == LGT) || (CUSTOMER == CCN) || (CUSTOMER == LON)
    GPIO_OutputControl(CAM_EN, TRUE);
    MMPF_OS_Sleep_MS(5);
    #endif

    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        GPIO_OutputControl(GPIO_SENSOR_PSEN, MMP_TRUE);
    	MMPF_OS_Sleep_MS(5);
        GPIO_OutputControl(GPIO_SENSOR_PSEN, MMP_FALSE);
    }
    else{
        MMPF_VIF_SetPIODir(VIF_SIF_SEN, MMP_TRUE);
        MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_TRUE);  //Pin output high
        MMPF_OS_Sleep_MS(5);
    }
    #if(BIND_SENSOR_OV5650)&&(AUDEXT_DAC==WM8960)
    if(SEN_AUD_EN==0){
        GPIO_OutputControl(SEN_PWR_EN, MMP_TRUE);
        SEN_AUD_EN=1;
    }else
        SEN_AUD_EN|=1;
    #else
    GPIO_OutputControl(SEN_PWR_EN, MMP_TRUE);
    #endif
    MMPF_OS_Sleep_MS(5);
    MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_FALSE);//Pin output low

    pVIF->VIF_SENSR_CLK_CTL[vif_id] = VIF_SENSR_CLK_EN;
    MMPF_OS_Sleep_MS(5);

    /*Reset Sensor 500mS*/
    MMPF_VIF_SetPIODir(VIF_SIF_RST, MMP_TRUE);

    MMPF_VIF_SetPIOOutput(VIF_SIF_RST, MMP_FALSE);
    MMPF_OS_Sleep_MS(5);

    MMPF_VIF_SetPIOOutput(VIF_SIF_RST, MMP_TRUE);
    MMPF_OS_Sleep_MS(10);

    SNR_OV5650_InitSensor_Customer();

    cur_res = GetResCfg(glPccamResolution);
    if(cur_res->res_w > gsSensorPreviewWidth[1] || cur_res->res_h > gsSensorPreviewHeight[1]) {
        if((cur_res->res_w <= gsSensorPreviewWidth[2]) && (cur_res->res_h <= gsSensorPreviewHeight[2])) {
            gsSensorFunction->MMPF_Sensor_SetPreviewMode(2);
        } else {
            gsSensorFunction->MMPF_Sensor_SetPreviewMode(0);
        }
    } else {
        gsSensorFunction->MMPF_Sensor_SetPreviewMode(1);
    }

#if SUPPORT_AUTO_FOCUS
    MOTOR_OV5650_VCM_InitMotor_Customer();
#endif

#if SUPPORT_FLASH_LIGHT
    VR_Flash_Init();
#endif

    RTNA_DBG_Str(0, "MMPF_Sensor_Initialize_OV5650...End\r\n");
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_PowerDown_OV5650
//  Description :
//------------------------------------------------------------------------------
//void VR_Motor_EnterStandby(void);

static MMP_ERR  MMPF_Sensor_PowerDown_OV5650(void)
{
    AITPS_VIF  pVIF = AITC_BASE_VIF;
    //AITPS_GPIO  pGPIO = AITC_BASE_GPIO;
    //AITPS_MIPI pMIPI = AITC_BASE_MIPI;
    //MMP_UBYTE   vif_id = 0;


    //gsSensorFunction->MMPF_Sensor_SetReg(0x0100,0);
    //MMPF_OS_Sleep_MS(33);

    MMPF_VIF_EnableInputInterface(MMP_FALSE);

#if SUPPORT_AUTO_FOCUS
    //VR_Motor_EnterStandby();
    //MMPF_MOTOR_WriteI2C((ISP_IF_AF_GetPos() >> 2) | 0x80, (ISP_IF_AF_GetPos() << 6) & 0xc0);
    //ISP_IF_AF_SetPos(0);
    //ISP_IF_MOTOR_GoToAFPos();    
    //gsSensorFunction->MMPF_Sensor_SetAFPosition(0);
    //MMPF_OS_Sleep_MS(10);
#endif

    //MMPF_VIF_SetPIODir(VIF_SIF_RST, MMP_TRUE);
    //MMPF_VIF_SetPIOOutput(VIF_SIF_RST, MMP_TRUE);  //Pin output high
    //MMPF_OS_Sleep_MS(10);
    MMPF_VIF_SetPIOOutput(VIF_SIF_RST, MMP_FALSE);//Pin output low
    MMPF_OS_Sleep_MS(10);

    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        GPIO_OutputControl(GPIO_SENSOR_PSEN, MMP_TRUE);  //Pin output high
    }
    else{
        MMPF_VIF_SetPIODir(VIF_SIF_SEN, MMP_TRUE);
        MMPF_VIF_SetPIOOutput(VIF_SIF_SEN, MMP_TRUE);  //Pin output high
    }
    MMPF_OS_Sleep_MS(5);
    #if(BIND_SENSOR_OV5650)&&(AUDEXT_DAC==WM8960)
    if(SEN_AUD_EN==1){
        GPIO_OutputControl(SEN_PWR_EN, MMP_FALSE);
        SEN_AUD_EN=0;
    }else
        SEN_AUD_EN&=0xFE;
    #else
    GPIO_OutputControl(SEN_PWR_EN, MMP_FALSE);
    #endif

    pVIF->VIF_SENSR_CLK_CTL[vif_id] &= (~ VIF_SENSR_CLK_EN) ;   //output sensor main clock

    #if (CUSTOMER == LGT) || (CUSTOMER == CCN) || (CUSTOMER == LON)
    GPIO_OutputControl(CAM_EN, FALSE);
    MMPF_OS_Sleep_MS(10);
    #endif

#if 0
    //RTNA_DBG_Str(3, "Config PS_GPIO pin as LOW\r\n");
    pGPIO->GPIO_DATA[3] &= ~(1 << 16);  // Turn Off Sensor Power
#endif
    MMPF_OS_Sleep_MS(20);

    RTNA_DBG_Str3( "MMPF_Sensor_PowerDown_OV5650() \r\n");
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetFrameRate_OV5650
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_SetFrameRate_OV5650(MMP_UBYTE ubFrameRate)
{
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetPreviewMode
//  Description :
//------------------------------------------------------------------------------

static MMP_ERR MMPF_Sensor_SetPreviewMode_OV5650(MMP_USHORT usPreviewmode)
{
    OV5650_Sensor_PreviewMode(usPreviewmode);

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetPreviewMode
//  Description :
//------------------------------------------------------------------------------

static MMP_ERR MMPF_Sensor_ChangePreviewMode_OV5650(
                        MMP_USHORT usCurPreviewmode, MMP_USHORT usCurPhasecount,
                        MMP_USHORT usNewPreviewmode, MMP_USHORT usNewPhasecount)
{
    OV5650_Sensor_PreviewMode(usNewPreviewmode);

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_ChangeMode
//  Description :
//------------------------------------------------------------------------------

/*static*/ MMP_ERR MMPF_Sensor_ChangeMode_OV5650(MMP_USHORT usPreviewmode, MMP_USHORT usCapturemode)
{
    switch (usPreviewmode) {
    case 0:
        switch (usCapturemode) {
        case 0:
            break;
        case 1:
            break;
        }
        break;
    case 1:
        switch (usCapturemode) {
        case 0:// SXGA preview, 5M capture
            MMPF_Sensor_SetPreviewMode_OV5650(0);
            // wait 3 frame ...
            // MMPF_Sensor_WaitFrame(5);
            break;
        case 1:
            MMPF_Sensor_SetPreviewMode_OV5650(1);
            // wait 3 frame ...
            //MMPF_Sensor_WaitFrame(3);
            break;
        case 2:
            MMPF_Sensor_SetPreviewMode_OV5650(2);
            // wait 3 frame ...
            //MMPF_Sensor_WaitFrame(3);
            break;
        }
        break;
    }

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_InitializeVIF
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_InitializeVIF_OV5650(void)
{
    AITPS_MIPI  pMIPI = AITC_BASE_MIPI;
    AITPS_VIF   pVIF = AITC_BASE_VIF;
    //MMP_UBYTE   vif_id = 0;
    ISP_HDM_VIF_IF vif_if;
    MMP_ULONG   ulSENSORCLK;

    // register sensor (functions and variables)
    //ISP_IF_SNR_Register();
    RTNA_DBG_Str(0, "ISP_IF_SNR_Register...E\r\n");

    #if SUPPORT_AUTO_FOCUS
    #if (CUSTOMER == NMG)  
    GPIO_OutputControl(AF_EN, TRUE);
    #endif
    #if (CUSTOMER == LGT)  
    //GPIO_OutputControl(AF_EN, FALSE);
    #endif
    #if (CUSTOMER == CCN)  
    //GPIO_OutputControl(AF_EN, FALSE);
    #endif
    MMPF_OS_Sleep_MS(20);
    #endif

#if SUPPORT_AUTO_FOCUS
    // register motor
    //ISP_IF_MOTOR_Register();
#endif

    #if (SENSOR_IF == SENSOR_IF_PARALLEL)
    MMPF_VIF_RegisterInputInterface(MMPF_VIF_PARALLEL);
    pVIF->VIF_SENSR_CTL[vif_id] = VIF_SENSR_LATCH_NEG | VIF_HSYNC_POLAR | VIF_12BPP_OUT_EN;
    #else
    #if 0
    pMIPI->MIPI_DATA1_SOT = 0x10; // From KW
    pMIPI->MIPI_DATA2_SOT = 0x10; // From KW
    pMIPI->MIPI_DATA3_SOT = 0x10; // From KW
    pMIPI->MIPI_DATA4_SOT = 0x10; // From KW
    #endif

    pMIPI->MIPI_DATA0_DLY[vif_id] = 0x1000;  // set SOT counter to 0x10
    pMIPI->MIPI_DATA1_DLY[vif_id] = 0x1000;  // set SOT counter to 0x10

    //#if (CUSTOMER == NMG)  
    //pMIPI->MIPI_CLK_DEL = 0x05;  // MIPI clock lane delay selection [3:0]
    //pMIPI->MIPI_CLK_CFG |= 0x02;  // MIPI clock lane delay
    //#endif
    MMPF_VIF_RegisterInputInterface(MMPF_VIF_MIPI);
    #endif

    // initialize VIF OPRs
#if (SENSOR_IF == SENSOR_IF_PARALLEL)
    vif_if = ISP_HDM_VIF_IF_PARALLEL;		//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_PARALLEL, 22);
#elif (SENSOR_IF == SENSOR_IF_MIPI_1_LANE)	
    vif_if = ISP_HDM_VIF_IF_MIPI_SINGLE_0;	//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_MIPI_SINGLE_0, 22);
#elif (SENSOR_IF == SENSOR_IF_MIPI_2_LANE)		
    vif_if = ISP_HDM_VIF_IF_MIPI_DUAL_01;	//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_MIPI_DUAL_01, 22);
#elif (SENSOR_IF == SENSOR_IF_MIPI_4_LANE)		
    vif_if = ISP_HDM_VIF_IF_MIPI_QUAD;		//ISP_HDM_IF_InitVIF(ISP_HDM_VIF_IF_MIPI_QUAD, 22);
#endif	

    {
        pVIF->VIF_INT_HOST_EN[vif_id] = 0;
        pVIF->VIF_INT_CPU_SR[vif_id] = VIF_INT_ALL;
        pVIF->VIF_OUT_EN[vif_id] = 0;
        //pVIF->VIF_DEFT_EN = 0;
        pVIF->VIF_RAW_OUT_EN[vif_id] = 0;
        pVIF->VIF_FRME_SKIP_NO[vif_id] = 0;
        pVIF->VIF_FRME_SKIP_EN[vif_id] = 0;

        //RTNA_DBG_Str(0,"InitializeVIF\r\n");
        MMPF_PLL_GetGroupFreq(4, &ulSENSORCLK);

        switch(ulSENSORCLK){
            case 96000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D4; // 24MHz
                RTNA_DBG_Str3("96MHz DIV 4\r\n");
                break;
            case 144000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D6; 
                RTNA_DBG_Str3("144MHz DIV 6\r\n");
                break;
            case 192000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D8;
                RTNA_DBG_Str3("192MHz DIV 8\r\n");
                break;
            case 216000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D9;
                RTNA_DBG_Str3("216MHz DIV 9\r\n");
                break;
            case 240000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D10;
                RTNA_DBG_Str3("240MHz DIV 10\r\n");
                break;
            case 120000:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D5;
                RTNA_DBG_Str3("120MHz DIV 5\r\n")
                break;
            default:
                pVIF->VIF_SENSR_CLK_FREQ[vif_id] = VIF_SENSR_CLK_PLL_D12;  // 144/12 = 12M
                break;
        }

        pVIF->VIF_SENSR_CTL[vif_id] = VIF_SENSR_LATCH_PST | VIF_LINE_ID_POLAR | VIF_PIXL_ID_POLAR | VIF_VSYNC_POLAR_NEG;
        pVIF->VIF_YUV_CTL[vif_id] = VIF_YUV_FORMAT_VUY;
        pVIF->VIF_SENSR_CLK_CTL[vif_id] = VIF_SENSR_CLK_EN;
        pVIF->VIF_OPR_UPD[vif_id] = VIF_OPR_UPD_EN | VIF_OPR_UPD_FRAME;

        if (vif_if==ISP_HDM_VIF_IF_PARALLEL) {
            RTNA_DBG_Str0("ISP_HDM_VIF_IF_PARALLEL\r\n");
        }
        else {
            pMIPI->MIPI_DATA0_CFG[vif_id] = ((pMIPI->MIPI_DATA0_CFG[vif_id] & ~MIPI_DAT_SRC_SEL_MASK) | MIPI_DAT_SRC_PHY_1) | MIPI_CSI2_EN;
            if (vif_if == ISP_HDM_VIF_IF_MIPI_DUAL_01) {
                pMIPI->MIPI_DATA1_CFG[vif_id] = ((pMIPI->MIPI_DATA1_CFG[vif_id] & ~MIPI_DAT_SRC_SEL_MASK) | MIPI_DAT_SRC_PHY_2) | MIPI_CSI2_EN;
            }
            #if (CHIP == P_V2)
            else if(vif_if==ISP_HDM_VIF_IF_MIPI_QUAD){
                pMIPI->MIPI_DATA2_CFG = MIPI_CSI2_EN | MIPI_SRC_SEL_2;
                pMIPI->MIPI_DATA3_CFG = MIPI_CSI2_EN | MIPI_SRC_SEL_3;
                pMIPI->MIPI_DATA4_CFG = MIPI_CSI2_EN | MIPI_SRC_SEL_4;
            }
            #endif
        }
    }

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_InitializeISP
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_InitializeISP_OV5650(void)
{
    // register sensor (functions and variables)
    RTNA_DBG_Str(0, "ISP_IF_LIB_Init...\r\n");
    // check if ISP Lib and IQ are mismatched.
    ISP_IF_LIB_Init();
    // check if ISP Lib and IQ are mismatched.
    if (ISP_IF_LIB_CompareIQVer() != MMP_ERR_NONE) {
        RTNA_DBG_Str(0, "Wrong ISP lib version!\r\n");
        return MMP_ERR_NONE;
    }

    //RTNA_DBG_Str(0, "ISP_IF_3A_Init...S\r\n");
    // initialize 3A
    ISP_IF_3A_Init();

    // set AF type (ISP_AF_TYPE_ONE_SHOT / ISP_AF_TYPE_CONTINUOUS)
    ISP_IF_AF_SetType(ISP_AF_TYPE_CONTINUOUS);
    //RTNA_DBG_Str(0, "ISP_IF_3A_Init...E\r\n");

    ISP_IF_AE_SetLowLuxMode(0);//disable Low light Compensation

    return  MMP_ERR_NONE;
}

//============================================================================

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_SetAFPosition_OV5650
//  Description :
// ---------------------------------------
static MMP_ERR MMPF_Sensor_SetAFPosition_OV5650(MMP_UBYTE ubPos)
{
#if SUPPORT_AUTO_FOCUS
    //if (gISPConfig.AutoFocusMode == VENUS_AF_MODE_MANUAL) {
        //gAFConfig.AFPos = ubPos;
        ISP_IF_AF_SetPos(ubPos, 8);
        MOTOR_OV5650_VCM_SetAFPosition_Customer(ISP_IF_AF_GetPos(0));
    //}

        //RTNA_DBG_Str(3, "  ubPos = ");       
        //RTNA_DBG_Short(3, ubPos);
        //RTNA_DBG_Str(3, "\r\n");
#endif
    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_Do3AOperation
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAWBOperation_OV5650(void)
{
//    ISP_IF_AWB_GetHWAcc(1);

    //ISP_IF_R_DoAWB();
    ISP_IF_AWB_Execute();
    ISP_IF_CALI_Execute();
    ISP_IF_IQ_SetAWBGains(ISP_IF_AWB_GetGainR(), ISP_IF_AWB_GetGainG(), ISP_IF_AWB_GetGainB());

    return  MMP_ERR_NONE;
}

//ISP_UINT32 gISPFrameCnt;
ISP_UINT32 gISPFrameCnt = 0;
#define FrameDivsor 5 // Gason@20120111, by OV suggestion, under 15 fps, dummy line will work every 5 frame.
ISP_UINT32 frame_div = 5;
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAEOperation_ST
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAEOperation_ST_OV5650(void)
{
    static ISP_UINT32 again, dgain, s_gain;

    if(ISP_IF_3A_GetSwitch(ISP_3A_ALGO_AE) != 1) return MMP_ERR_NONE;

    //switch (frame_cnt++ % 6) {
    switch (gISPFrameCnt % frame_div) {
    case 0:
        ISP_IF_AE_Execute();
        {
            ISP_UINT32 H, L;

            again = ISP_IF_AE_GetAGain();
            dgain = ISP_IF_AE_GetDGain();

            //calc sensor gain
            s_gain = again * dgain / ISP_IF_AE_GetDGainBase();

            if (s_gain >= ISP_IF_AE_GetAGainBase() * 8) {
                dgain =s_gain* ISP_IF_AE_GetDGainBase() /(ISP_IF_AE_GetAGainBase() *8);           
                H = 7;
            } else if (s_gain >= ISP_IF_AE_GetAGainBase() * 4) {
                dgain =s_gain* ISP_IF_AE_GetDGainBase() /(ISP_IF_AE_GetAGainBase() *4);           
                H = 3;
            } else if (s_gain >= ISP_IF_AE_GetAGainBase() * 2) {
                dgain =s_gain* ISP_IF_AE_GetDGainBase() /(ISP_IF_AE_GetAGainBase() *2);           
                H = 1;
            } else {
                dgain =s_gain* ISP_IF_AE_GetDGainBase() /(ISP_IF_AE_GetAGainBase() *1);           
                H = 0;
            }
			
            L = (dgain - ISP_IF_AE_GetDGainBase())  * 16 / ISP_IF_AE_GetDGainBase();

            if (L > 15) L = 15;

            //gsSensorFunction->MMPF_Sensor_SetReg(0x350A, (sensor_gain >> 8) & 0x01);
            gsSensorFunction->MMPF_Sensor_SetReg(0x350B, (H<<4)+L);

            ISP_IF_AE_SetAGain(((1+(H&4)/4)*(1+(H&2)/2)*(1+(H&1))*(L+16))*ISP_IF_AE_GetAGainBase()/16);
            ISP_IF_AE_SetDGain(ISP_IF_AE_GetDGainBase());
            ISP_IF_IQ_SetAEGain(ISP_IF_AE_GetDGainBase());

            //s_gain = s_gain * 16 / ISP_IF_AE_GetAGainBase();
            //MMPF_Sensor_SetReg_OV5650(0x350B, s_gain);
            //gsSensorFunction->MMPF_Sensor_SetReg(0x350B, s_gain);
            //MMPF_Sensor_SetSensorShutter_OV5650(ISP_IF_AE_GetShutter(), ISP_IF_AE_GetVsync());
            gsSensorFunction->MMPF_Sensor_SetSensorShutter(ISP_IF_AE_GetShutter(), ISP_IF_AE_GetVsync());
        }
        break;
    case 1:
        //ISP_IF_AE_SetDGain(dgain);
        //ISP_IF_IQ_SetAEGain(dgain);
        break;
    }

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAEOperation_END
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAEOperation_END_OV5650(void)
{
    if(ISP_IF_3A_GetSwitch(ISP_3A_ALGO_AE) != 1) return MMP_ERR_NONE;
    ISP_IF_AWB_GetHWAcc(1);
    ISP_IF_AE_GetHWAcc(1);

    switch (gISPFrameCnt++ % frame_div) {
    //case 1:
    //	ISP_IF_IQ_SetAEGain(ISP_IF_AE_GetDGainBase());
    //	break;
    case 2:
        break;
    }

    return  MMP_ERR_NONE;
}


//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAFOperation
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAFOperation_OV5650(void)
{
#if SUPPORT_AUTO_FOCUS
    if(gbAutoFocus == MMP_TRUE){
        //VR_AF_GetAcc();
        ISP_IF_AF_GetHWAcc(1);
        //ISP_IF_R_DoAF();
    }
#endif

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoAFOperation
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_Sensor_DoAFOperation_FrameStart_OV5650(void)
{
#if SUPPORT_AUTO_FOCUS
    if(gbAutoFocus == MMP_TRUE){
        //VR_AF_GetAcc();
        //ISP_IF_R_GetAFAcc();
        //ISP_IF_R_DoAF();
        {
            static ISP_UINT32 frame_cnt = 0;

            switch (frame_cnt++ % 1) {
            case 0:
                ISP_IF_AF_Execute();
                MOTOR_OV5650_VCM_SetAFPosition_Customer(ISP_IF_AF_GetPos(0));
                break;
            }
        }
    }
#endif

    return  MMP_ERR_NONE;
}

//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_DoIQOperation_OV5650
//  Description :
//------------------------------------------------------------------------------
//extern MMP_UBYTE gbSkypeMode;
extern H264_FORMAT_TYPE gbCurH264Type;
static MMP_ERR  MMPF_Sensor_DoIQOperation_OV5650(void)
{
    // switch IQ table if video format is H264
    //PCAM_USB_VIDEO_FORMAT vidFmt = pcam_get_info()->pCamVideoFormat;

    //if (vidFmt == PCAM_USB_VIDEO_FORMAT_H264)	ISP_IF_F_SetVideoMode(1);
    //else										ISP_IF_F_SetVideoMode(0);


    // set IQ at frame end to ensure frame sync	
    ISP_IF_IQ_SetAll();
#if SUPPORT_ISP_ENGINEERING_MODE_FUNCTION
    {
        extern MMP_USHORT PCAM_UpdateGAMMAtoRAM(void);
        PCAM_UpdateGAMMAtoRAM();
    }
#endif
    ISP_IF_IQ_CheckBypass();

    //RTNA_DBG_Str(0, "DoIQOperation_OV5650()\r\n");

    return  MMP_ERR_NONE;
}

// Steven ADD
//------------------------------------------------------------------------------
//  Function    : MMPF_SetLightFreq
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR  MMPF_SetLightFreq_OV5650(MMP_USHORT usMode)
{
#if 0

    RTNA_DBG_Str(0, "MMPF_SetLightFreq_OV5650 : ");
    RTNA_DBG_Short(0, usMode);
    RTNA_DBG_Str(0, "\r\n");

    /*
    typedef enum {
        VENUS_FLICKER_MODE_OFF              = 0,
        VENUS_FLICKER_MODE_AUTO             = 1,
        VENUS_FLICKER_MODE_60HZ             = 2,
        VENUS_FLICKER_MODE_50HZ             = 3,
        VENUS_FLICKER_MODE_CAL_BASESHUTTER  = 4
    } VENUS_FLICKER_MODE;
    */

    switch(usMode) {
    case 0:
    case 2:
        //gISPConfig.FlickerMode = VENUS_FLICKER_MODE_60HZ;
        ISP_IF_AE_SetFlicker(ISP_AE_FLICKER_60HZ);
        break;
    case 1:
    case 3:
        //gISPConfig.FlickerMode = VENUS_FLICKER_MODE_50HZ;
        ISP_IF_AE_SetFlicker(ISP_AE_FLICKER_50HZ);
        break;
    default:
        //gISPConfig.FlickerMode = VENUS_FLICKER_MODE_AUTO;
        ISP_IF_AE_SetFlicker(ISP_AE_FLICKER_AUTO);
    }
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_SetStableState_OV5650(MMP_BOOL bStableState)
{
    return  MMP_ERR_NONE;
}

MMP_ERR MMPF_SetHistEQState_OV5650(MMP_BOOL bHistEQState)
{
    return  MMP_ERR_NONE;
}

extern volatile MMP_UBYTE gbDramID;
static MMP_ERR  MMPF_Sensor_SetReg_OV5650(MMP_USHORT usAddr, MMP_USHORT usData)
{
    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_0;  // for AIT8433 70 pin package
    }
    else{
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_1;  // for AIT8451 105 pin package
    }
    MMPF_I2cm_WriteReg(&m_snr_I2cmAttribute, usAddr, usData);
    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_GetReg_OV5650(MMP_USHORT usAddr, MMP_USHORT *usData)
{
    *usData = 0xFF;
    if((gbDramID == MMPF_DRAMID_WINBOND_SDR16) || (gbDramID == MMPF_DRAMID_WINBOND_SDR16_1_8V)){
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_0;  // for AIT8433 70 pin package
    }
    else{
        m_snr_I2cmAttribute.uI2cmID = MMPF_I2CM_ID_1;  // for AIT8451 105 pin package
    }
    MMPF_I2cm_ReadReg(&m_snr_I2cmAttribute, usAddr, usData);

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetImageScene_OV5650(MMP_USHORT imgSceneID)
{
#if 0
/*
typedef enum {
    ISP_SCENE_AUTO					= 0,	// scene mode
    ISP_SCENE_PORTRAIT				= 1,	// scene mode
    ISP_SCENE_LANDSCAPE				= 2,	// scene mode
    ISP_SCENE_SPORTS				= 3,	// scene mode
    ISP_SCENE_SUNSET				= 4,	// scene mode
    ISP_SCENE_DUSK					= 5,	// scene mode
    ISP_SCENE_DAWN					= 6,	// scene mode
    ISP_SCENE_NIGHT_SHOT			= 7,	// scene mode
    ISP_SCENE_AGAINST_LIGHT			= 8,	// scene mode
    ISP_SCENE_TEXT					= 9,	// scene mode
    ISP_SCENE_MANUAL				= 10,	// scene mode
    ISP_SCENE_INDOOR				= 11,	// scene mode
    ISP_SCENE_SNOW					= 12,	// scene mode
    ISP_SCENE_FALL					= 13,	// scene mode
    ISP_SCENE_WAVE					= 14,	// scene mode
    ISP_SCENE_FIREWORKS				= 15,	// scene mode
    ISP_SCENE_SHOW_WIN				= 16,	// scene mode
    ISP_SCENE_CANDLE				= 17,	// scene mode
    ISP_SCENE_NONE					= 18	// camera mode
} ISP_SCENE;
*/
    imgSceneID = (imgSceneID % (ISP_SCENE_NONE+1));

    RTNA_DBG_Str(3, "MMPF_SetImageScene_OV5650 : ");
    RTNA_DBG_Byte(3, imgSceneID);
    RTNA_DBG_Str(3, "\r\n");

    //if (g3AConfig.Control3A == ISP_3A_ENABLE)
        MMPF_Sensor_CheckFrameEnd(1);
    ISP_IF_F_SetScene(imgSceneID);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetAEMode_OV5650(MMP_UBYTE ubAEMode, MMP_UBYTE ubISOMode)
{
#if 0
/*
typedef enum {
    AE_MODE_AUTO						= 0,
    AE_MODE_MANUAL						= 1,
    AE_MODE_ISO							= 2,
    AE_MODE_SHUTTER						= 3
} VENUS_AE_MODE;
*/
/*
typedef enum {
    AE_ISO_AUTO					= 0,
    AE_ISO_50					= 1,
    AE_ISO_100					= 2,
    AE_ISO_200					= 3,
    AE_ISO_400					= 4,
    AE_ISO_800					= 5,
    AE_ISO_1600					= 6,
    AE_ISO_3200					= 7,
    ISP_AE_ISO_NUM
} VENUS_AE_ISO;
*/
    #if 0 //FIXME, VSN_V2
    if (ubAEMode == 3)
        ISP_IF_AE_SetISO(ISP_AE_MODE_AUTO);
    else
    #endif
        ISP_IF_AE_SetISO(ubISOMode);
#endif
	
    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetAFMode_OV5650(MMP_UBYTE ubAFMode)
{
#if 0
/*
typedef enum {
    VENUS_AF_MODE_AUTO					= 0,
    VENUS_AF_MODE_MANUAL				= 1,
    VENUS_AF_MODE_MACRO					= 2,
    VENUS_AF_MODE_FULL					= 3,
    VENUS_AF_MODE_MOTOR_TEST            = 4,
    VENUS_AF_MODE_NULL					= 5
} VENUS_AF_MODE;
*/
    RTNA_DBG_Str(3, "MMPF_Sensor_SetAFMode_OV5650 : ");
    RTNA_DBG_Byte(3, ubAFMode);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.AutoFocusMode = (MMP_UBYTE)ubAFMode;
    //if (g3AConfig.Control3A == ISP_3A_ENABLE)
    //	MMPF_Sensor_CheckFrameEnd(1);
    ISP_IF_AF_SetMode(ubAFMode);
#endif

    return  MMP_ERR_NONE;
}

//======================================================================//
// function : MMPF_Sensor_SetImageEffect                                //
// parameters:                                                          //
// return :                                                             //
// description :                                                        //
//======================================================================//
static MMP_ERR  MMPF_Sensor_SetImageEffect_OV5650(MMP_USHORT imgEffectID)
{
#if 0
/*
typedef enum {
    ISP_IMAGE_EFFECT_NORMAL			= 0,
    ISP_IMAGE_EFFECT_GREY			= 1,
    ISP_IMAGE_EFFECT_SEPIA			= 2,
    ISP_IMAGE_EFFECT_NEGATIVE		= 3,
    ISP_IMAGE_EFFECT_ANTIQUE		= 4,
    ISP_IMAGE_EFFECT_WATERCOLOR		= 5,
    ISP_IMAGE_EFFECT_PORTRAIT		= 6,
    ISP_IMAGE_EFFECT_LANDSCAPE		= 7,
    ISP_IMAGE_EFFECT_SUNSET			= 8,
    ISP_IMAGE_EFFECT_DUSK			= 9,
    ISP_IMAGE_EFFECT_DAWN			= 10,
    ISP_IMAGE_EFFECT_RED			= 11,
    ISP_IMAGE_EFFECT_GREEN			= 12,
    ISP_IMAGE_EFFECT_BLUE			= 13,
    ISP_IMAGE_EFFECT_YELLOW			= 15,
    ISP_IMAGE_EFFECT_EMBOSS			= 17,
    ISP_IMAGE_EFFECT_OIL			= 18,
    ISP_IMAGE_EFFECT_BW				= 19,
    ISP_IMAGE_EFFECT_SKETCH			= 20,
    ISP_IMAGE_EFFECT_CRAYONE		= 21,
    ISP_IMAGE_EFFECT_WHITEBOARD		= 22,
    ISP_IMAGE_EFFECT_BLACKBOARD		= 23,
    ISP_IMAGE_EFFECT_VIVID			= 24,
    ISP_IMAGE_EFFECT_NUM
} ISP_IMAGE_EFFECT;
*/

    imgEffectID = (imgEffectID % (ISP_IMAGE_EFFECT_NUM+1));

    RTNA_DBG_Str(3, "MMPF_Sensor_SetImageEffect_OV5650 : ");
    RTNA_DBG_Byte(3, imgEffectID);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.ImageEffect = (MMP_USHORT)imgEffectID;
    //if (g3AConfig.Control3A == ISP_3A_ENABLE)
    //	MMPF_Sensor_CheckFrameEnd(1);
    ISP_IF_F_SetImageEffect(imgEffectID);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetAWBType_OV5650(MMP_UBYTE ubType)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetAWBType_OV5650 : ");
    RTNA_DBG_Byte(3, ubType);
    RTNA_DBG_Str(3, "\r\n");

    cAWBtype = ubType;
#endif
    return  MMP_ERR_NONE;
}
static MMP_ERR  MMPF_Sensor_SetContrast_OV5650(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_Contrast_OV5650 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.ContrastLevel = ubLevel;
    ISP_IF_F_SetContrast(ubLevel);
#endif

    return  MMP_ERR_NONE;
}
static MMP_ERR  MMPF_Sensor_SetSaturation_OV5650(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetSaturation_OV5650 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.SaturationLevel = ubLevel;
    ISP_IF_F_SetSaturation(ubLevel);
#endif

    return  MMP_ERR_NONE;
}
static MMP_ERR  MMPF_Sensor_SetSharpness_OV5650(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetSharpness_OV5650 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //gISPConfig.SharpnessLevel = ubLevel;
    ISP_IF_F_SetSharpness(ubLevel);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetHue_OV5650(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetHue_OV5650 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    ISP_IF_F_SetHue(ubLevel);
#endif

    return  MMP_ERR_NONE;
}

static MMP_ERR  MMPF_Sensor_SetGamma_OV5650(MMP_SHORT ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetGamma_OV5650 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    ISP_IF_F_SetGamma(ubLevel);
#endif

    return  MMP_ERR_NONE;
}

/*static MMP_ERR  MMPF_Sensor_SetBacklight_OV5650(MMP_UBYTE ubLevel)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetBacklight_OV5650 : ");
    RTNA_DBG_Byte(3, ubLevel);
    RTNA_DBG_Str(3, "\r\n");

    //ISP_IF_F_SetBacklight(ubLevel);
    if(ubLevel > 1){
        ISP_IF_F_SetWDR(ISP_WDR_DISABLE);
    }
    else{
        ISP_IF_F_SetWDR(ISP_WDR_ENABLE);
    }
#endif

    return  MMP_ERR_NONE;
}
*/

static void  MMPF_Sensor_SetCaptureISPSetting_OV5650(MMP_UBYTE usCaptureBegin)
{
#if 0
    AITPS_VIF   pVIF = AITC_BASE_VIF;
    MMP_ULONG   ulVIF_Width, ulVIF_Height;
    //MMP_UBYTE   vid = 0;

    /* ToDo: setting IOS and ISP windows*/
    if(MMP_TRUE == usCaptureBegin){
        ulVIF_Width = (pVIF->VIF_GRAB[vif_id].PIXL_ED - pVIF->VIF_GRAB[vif_id].PIXL_ST + 1);
        ulVIF_Height = (pVIF->VIF_GRAB[vif_id].LINE_ED - pVIF->VIF_GRAB[vif_id].LINE_ST + 1);
        //gSensorFunc.SetISPWindows(ulVIF_Width,ulVIF_Height);
    }
#endif
	
    return;
}

/*Prepare for raw preview zoom*/
static void  MMPF_Sensor_SetISPWindow_OV5650(MMP_ULONG ulWidth, MMP_ULONG ulHeight)
{
    //gSensorFunc.SetISPWindows(ulWidth,ulHeight);
    return;
}

/*Check AIT can preview in this resolution*/
static MMP_UBYTE  MMPF_Sensor_CheckPreviewAbility_OV5650(MMP_USHORT usPreviewmode)
{
#if 0
    switch(usPreviewmode){
        case 0:                 //5M can't preview
            return MMP_TRUE;
            break;
        case 1:                 //1.3M can preview
            return MMP_TRUE;
            break;
    }
#endif
	
    return MMP_TRUE;            //5M and 1.3M always can preview
}

static void SetAFWin_OV5650(MMP_USHORT usIndex, MMP_USHORT usPreviewWidth, MMP_USHORT usPreviewHeight,
                    MMP_USHORT  usStartX, MMP_USHORT  usStartY, MMP_USHORT  usWidth, MMP_USHORT  usHeight)
{
    //DBG_S3("SetAFWin (TODO)\r\n");
}

//static MMP_BOOL VGAPreview = MMP_TRUE;
static void SetAFEnable_OV5650(MMP_UBYTE enable)
{
    //if (enable) {
        //RTNA_DBG_Str(3, "VENUS_ISP_AUTO_FOCUS_STATUS_START\r\n");
        ISP_IF_AF_Control(ISP_AF_START);
        //actually calling SNR_OV5650_SetAutoFocusControl()
    //} else {
        //RTNA_DBG_Str(3, "VENUS_ISP_AUTO_FOCUS_STATUS_STOP\r\n");
        //ISP_IF_AF_Control(ISP_AF_STOP);
    //}
}

static MMP_ERR MMPF_Sensor_SetAEEnable_OV5650(MMP_UBYTE bEnable)
{
    /*
    if (bEnable) {
        g3AConfig.AE_Enabled     = ON;
    }
    else {
        g3AConfig.AE_Enabled     = OFF;
    }*/

    ISP_IF_3A_SetSwitch(ISP_3A_ALGO_AE, bEnable);

    return MMP_ERR_NONE;
}

static MMP_ERR MMPF_Sensor_SetAWBEnable_OV5650(MMP_UBYTE bEnable)
{
#if 0
    if (bEnable) {
        g3AConfig.AWB_Enabled    = ON;
        g3AConfig.AWBRGB_Enabled = ON;
    }
    else {
        g3AConfig.AWB_Enabled    = OFF;
        g3AConfig.AWBRGB_Enabled = OFF;
    }
#endif

    if(bEnable)
        ISP_IF_AWB_SetMode(ISP_AWB_MODE_AUTO);
    else
        ISP_IF_AWB_SetMode(ISP_AWB_MODE_MANUAL);

    return MMP_ERR_NONE;
}
static void SetAEsmooth_OV5650(MMP_UBYTE smooth)  {return;}
static MMP_ERR MMPF_Sensor_SetExposureValue_OV5650(MMP_UBYTE ev)
{
#if 0
    RTNA_DBG_Str(3, "MMPF_Sensor_SetExposureValue_OV5650 : ");
    //RTNA_DBG_Byte(3, gISPConfig.EV);
    //RTNA_DBG_Str(3, " : ");
    RTNA_DBG_Byte(3, ev);
    RTNA_DBG_Str(3, "\r\n");

    ISP_IF_AE_SetEV(ev);
#endif

    return MMP_ERR_NONE;
}

static MMP_UBYTE GetAFPosition_OV5650     (void){ return 0;/*volatile MMP_UBYTE *GPIO_BASE_B = (volatile MMP_UBYTE *)0x80009400;  return GPIO_BASE_B[0xB0];*/}


static MMP_UBYTE GetAFEnable_OV5650(void)
{
    //return 0;
    return ISP_IF_AF_GetResult();
}

static MMP_UBYTE GetExposureValue_OV5650  (void){return ISP_IF_AE_GetEV();}
static MMP_UBYTE GetAEsmooth_OV5650       (void){return 0;}
//------------------------------------------------------------------------------
//  Function    : MMPF_Sensor_Set3AStatus
//  Description :
//------------------------------------------------------------------------------
static MMP_ERR	MMPF_Sensor_Set3AStatus_OV5650(MMP_BOOL bEnable)
{
    #if 1
    MMPF_Sensor_3A_SetInterrupt(bEnable);
    //#else
    if (bEnable)
        ISP_IF_3A_Control(ISP_3A_ENABLE);
    else
        ISP_IF_3A_Control(ISP_3A_DISABLE);
    #endif
    return	MMP_ERR_NONE;
}

static void  MMPF_Sensor_SetColorID_OV5650(MMPF_SENSOR_ROTATE_TYPE RotateType)
{
    MMP_USHORT read_value;

    switch(RotateType) {
    case MMPF_SENSOR_ROTATE_NO_ROTATE:
        MMPF_Sensor_GetReg_OV5650(0x3818, &read_value);
        read_value = read_value & ~(0x60);
        MMPF_Sensor_SetReg_OV5650(0x3818, read_value);

        MMPF_Sensor_GetReg_OV5650(0x3621, &read_value);
        read_value = read_value | (0x10);
        MMPF_Sensor_SetReg_OV5650(0x3621, read_value);

        MMPF_Sensor_SetReg_OV5650(0x3827, 0x0a);//bit 0, 1
        MMPF_Sensor_SetReg_OV5650(0x3803, 0x0a);//bit 0
        MMPF_Sensor_SetReg_OV5650(0x505a, 0x00);
        MMPF_Sensor_SetReg_OV5650(0x505b, 0x12);
        ISP_IF_IQ_SetColorID(0);
        break;
    case MMPF_SENSOR_ROTATE_RIGHT_90:
        break;
    case MMPF_SENSOR_ROTATE_RIGHT_180:
        MMPF_Sensor_GetReg_OV5650(0x3818, &read_value);
        read_value = read_value | (0x60);
        MMPF_Sensor_SetReg_OV5650(0x3818, read_value);
        
        MMPF_Sensor_GetReg_OV5650(0x3621, &read_value);
        read_value = read_value & ~(0x10);
        MMPF_Sensor_SetReg_OV5650(0x3621, read_value);
        
        MMPF_Sensor_SetReg_OV5650(0x3827, 0x0b);//bit 0, 1 ***
        MMPF_Sensor_SetReg_OV5650(0x3803, 0x0b);//bit 0
        MMPF_Sensor_SetReg_OV5650(0x505a, 0x0a);
        MMPF_Sensor_SetReg_OV5650(0x505b, 0x2e);
    	ISP_IF_IQ_SetColorID(3);
    	break;
    case MMPF_SENSOR_ROTATE_RIGHT_270:
        break;
    case MMPF_SENSOR_ROTATE_H_MIRROR:
        MMPF_Sensor_GetReg_OV5650(0x3818, &read_value);
        read_value = ((read_value & ~(0x60)) | 0x40);
        MMPF_Sensor_SetReg_OV5650(0x3818, read_value);

        MMPF_Sensor_GetReg_OV5650(0x3621, &read_value);
        read_value = read_value & ~(0x10);
        MMPF_Sensor_SetReg_OV5650(0x3621, read_value);

        MMPF_Sensor_SetReg_OV5650(0x3827, 0x0a);//bit 0, 1
        MMPF_Sensor_SetReg_OV5650(0x3803, 0x0a);//bit 0***
        MMPF_Sensor_SetReg_OV5650(0x505a, 0x0a);
        MMPF_Sensor_SetReg_OV5650(0x505b, 0x2e);
        ISP_IF_IQ_SetColorID(1);
        break;
    case MMPF_SENSOR_ROTATE_V_FLIP:
        MMPF_Sensor_GetReg_OV5650(0x3818, &read_value);
        read_value = ((read_value & ~(0x60)) | 0x20);
        MMPF_Sensor_SetReg_OV5650(0x3818, read_value);

        MMPF_Sensor_GetReg_OV5650(0x3621, &read_value);
        read_value = read_value | (0x10);
        MMPF_Sensor_SetReg_OV5650(0x3621, read_value);

        MMPF_Sensor_SetReg_OV5650(0x3827, 0x0b);
        MMPF_Sensor_SetReg_OV5650(0x3803, 0x0b);//bit 0
        MMPF_Sensor_SetReg_OV5650(0x505a, 0x00);
        MMPF_Sensor_SetReg_OV5650(0x505b, 0x12);

        ISP_IF_IQ_SetColorID(2);
        break;
    }

    ISP_IF_IQ_SetDirection((ISP_UINT8)RotateType);
}

static void MMPF_Sensor_SetSensorGain_OV5650(MMP_ULONG gain)
{
    MMP_USHORT s_gain;

    s_gain = 16 * VR_MAX(gain, ISP_IF_AE_GetAGainBase()) / ISP_IF_AE_GetAGainBase();
    MMPF_Sensor_SetReg_OV5650(0x350A, (s_gain >> 8) & 0x01);
    MMPF_Sensor_SetReg_OV5650(0x350B, s_gain & 0xff);

}

static void MMPF_Sensor_SetSensorShutter_OV5650(MMP_ULONG shutter, MMP_ULONG vsync)
{
    ISP_UINT32 new_vsync =  VR_MAX(vsync, shutter + 4);  //+8
    ISP_UINT32 new_shutter = VR_MIN(VR_MAX(shutter, 1), new_vsync - 4);

    //dbg_printf(3,"v=0x%x, s=0x%x\r\n", new_vsync, new_shutter);

    MMPF_Sensor_SetReg_OV5650(0x380E, new_vsync >> 8);
    MMPF_Sensor_SetReg_OV5650(0x380F, new_vsync);
    MMPF_Sensor_SetReg_OV5650(0x3500, (ISP_UINT8)((new_shutter >> 12)&0xff));
    MMPF_Sensor_SetReg_OV5650(0x3501, (ISP_UINT8)((new_shutter >> 4)&0xff));
    MMPF_Sensor_SetReg_OV5650(0x3502, (ISP_UINT8)((new_shutter << 4)&0xff));

}

MMPF_SENSOR_FUNCTION  SensorFunction_OV5650 =
{
    MMPF_Sensor_Initialize_OV5650,
    MMPF_Sensor_InitializeVIF_OV5650,
    MMPF_Sensor_InitializeISP_OV5650,
    MMPF_Sensor_PowerDown_OV5650,
    MMPF_Sensor_SetFrameRate_OV5650,
    MMPF_Sensor_ChangeMode_OV5650,
    MMPF_Sensor_ChangePreviewMode_OV5650,
    MMPF_Sensor_SetPreviewMode_OV5650,
    MMPF_Sensor_SetReg_OV5650,
    MMPF_Sensor_GetReg_OV5650,
    MMPF_Sensor_DoAWBOperation_OV5650,
    MMPF_Sensor_DoAEOperation_ST_OV5650,
    MMPF_Sensor_DoAEOperation_END_OV5650,
    MMPF_Sensor_DoAFOperation_OV5650,
    MMPF_Sensor_DoAFOperation_FrameStart_OV5650,
    MMPF_Sensor_DoIQOperation_OV5650,
    MMPF_Sensor_SetImageEffect_OV5650,
    MMPF_SetLightFreq_OV5650,
    MMPF_SetStableState_OV5650,
    MMPF_SetHistEQState_OV5650,
    MMPF_Sensor_SetAFPosition_OV5650,
    SetAFWin_OV5650,
    SetAFEnable_OV5650,
    MMPF_Sensor_SetAEEnable_OV5650,
    MMPF_Sensor_SetAWBEnable_OV5650,
    MMPF_Sensor_SetExposureValue_OV5650,
    SetAEsmooth_OV5650,
    MMPF_Sensor_SetImageScene_OV5650,
    GetAFPosition_OV5650,
    GetAFEnable_OV5650,
    GetExposureValue_OV5650,
    GetAEsmooth_OV5650,
    MMPF_Sensor_SetAWBType_OV5650,
    MMPF_Sensor_SetContrast_OV5650,
    MMPF_Sensor_SetSaturation_OV5650,
    MMPF_Sensor_SetSharpness_OV5650,
    MMPF_Sensor_SetHue_OV5650,
    MMPF_Sensor_SetGamma_OV5650,
    //MMPF_Sensor_SetBacklight_OV5650,
    MMPF_Sensor_SetAEMode_OV5650,
    MMPF_Sensor_SetAFMode_OV5650,
    MMPF_Sensor_SetCaptureISPSetting_OV5650,
    MMPF_Sensor_SetISPWindow_OV5650,
    MMPF_Sensor_CheckPreviewAbility_OV5650,
    MMPF_Sensor_Set3AStatus_OV5650,
    MMPF_Sensor_SetColorID_OV5650,
    MMPF_Sensor_SetSensorGain_OV5650,
    MMPF_Sensor_SetSensorShutter_OV5650
};


MMP_USHORT SNR_OV5650_Reg_Init_Customer[] = 
{
    0x3103, 0x42,
    0x3103, 0x93,
    0x3b07, 0x0c,
    0x3017, 0xff,
    0x3018, 0xfc,
    0x3706, 0x41,
    0x3630, 0x22,
    0x3605, 0x04,
    0x3606, 0x3f,
    0x3712, 0x13,
    0x370e, 0x00,
    0x370b, 0x40,
    0x3600, 0x54,
    0x3601, 0x05,
    0x3631, 0x22,
    0x3612, 0x1a,
    0x3604, 0x40,
    0x3710, 0x28,
    0x3702, 0x3a,
    0x3704, 0x18,
    0x3a18, 0x00,
    0x3a19, 0xf8,
    0x3a00, 0x38,
    0x3830, 0x50,
    0x3a08, 0x12,
    0x3a09, 0x70,
    0x3a0a, 0x0f,
    0x3a0b, 0x60,
    0x3a0d, 0x06,
    0x3a0e, 0x06,
    0x3a13, 0x54,
    0x3815, 0x82,
    0x5059, 0x80,
    0x3615, 0x52,
    0x3a08, 0x16,
    0x3a09, 0x48,
    0x3a0a, 0x12,
    0x3a0b, 0x90,
    0x3a0d, 0x03,
    0x3a0e, 0x03,
    0x3a1a, 0x06,
    0x3623, 0x01,
    0x3633, 0x24,
    0x3c01, 0x34,
    0x3c04, 0x28,
    0x3c05, 0x98,
    0x3c07, 0x07,
    0x3c09, 0xc2,
    0x4000, 0x05,
    0x401d, 0x28,
    0x4001, 0x02,
    0x401c, 0x46,
    0x5046, 0x01,
    0x3810, 0x40,
    0x3836, 0x41,
    0x505f, 0x04,
    0x5000, 0x06,//defect enable //0x00,
    0x5001, 0x00,
    0x503d, 0x00,
    0x585a, 0x01,
    0x585b, 0x2c,
    0x585c, 0x01,
    0x585d, 0x93,
    0x585e, 0x01,
    0x585f, 0x90,
    0x5860, 0x01,
    0x5861, 0x0d,
    0x5180, 0xc0,
    0x5184, 0x00,
    0x470a, 0x00,
    0x470b, 0x00,
    0x470c, 0x00,
    0x300f, 0x8e,
    0x3603, 0xa7,
    0x3632, 0x55,
    0x3620, 0x56,
    0x3631, 0x36,
    0x3632, 0x5f,
    0x3711, 0x24,
    0x401f, 0x03,
    0x3010, 0x10,
    0x3011, 0x10,
    0x3406, 0x01,
    0x3400, 0x04,
    0x3401, 0x00,
    0x3402, 0x04,
    0x3403, 0x00,
    0x3404, 0x04,
    0x3405, 0x00,
    0x3503, 0x37,//0x17,
    0x3500, 0x00,
    0x3501, 0x18,
    0x3502, 0x00,
    0x350a, 0x00,
    0x350b, 0x38,
    0x4704, 0x00,
    0x4708, 0x01,
    0x3008, 0x02,

    //mipi
    0x3007, 0x3B,
    0x4801, 0x0F,
    0x3003, 0x03,
    0x300e, 0x0C,
    0x4803, 0x50,
    0x4800, 0x04,
    0x300f, 0x8F,
    0x3815, 0x82,
    0x3003, 0x01,
    //end of mipi
};

void SNR_OV5650_InitSensor_Customer(void) 
{
    // implement your initialize sensor routine here ==============================================

    ISP_UINT32 i;

    //MMPF_Sensor_ConfigI2C( SENSOR_I2C_ADDR_OV5650, ISP_I2C_TYPE_2A1D);

    //ISP_HDM_IF_EnableSensor(ISP_HDM_SNR_ENABLE_HIGH);

    // set register...........
    RTNA_DBG_Str(0, "set sensor reg\r\n");
    for (i = 0; i < sizeof(SNR_OV5650_Reg_Init_Customer)/4; i++) {
        gsSensorFunction->MMPF_Sensor_SetReg(SNR_OV5650_Reg_Init_Customer[i*2], SNR_OV5650_Reg_Init_Customer[i*2+1]);
        //MMPF_OS_Sleep_MS(3);
    }
    MMPF_OS_Sleep_MS(20);
    //ISP_IF_AE_SetBaseShutter( BASE_SHUTTER_50HZ, BASE_SHUTTER_60HZ);

    // Set preview resolution as default
    //SNR_OV5650_SetSensorResolution_Customer(ISP_SENSOR_RESOL_1280x720);
    //ISP_IF_AE_SetMinFPSx10(100);//@20120824
}

void SNR_OV5650_SetSensorAddMode_Customer(ISP_UINT8 enable)
{

}

MMP_USHORT SNR_OV5650_Reg_2560x1920_Customer[] = 
{
    //2600x1960 15fps ok
    0x3800, 0x02, // HREF Horizontal Start Point High Byte [3:0]
    0x3801, 0x22, // HREF Horizontal Start Point Low Byte
    0x3803, 0x0C, // HREF Vertical Start Point Low Byte ==> 0x10

    0x3804, 0x0A, // HREF width = 2600
    0x3805, 0x28,
    0x3806, 0x07, // VREF height = 1960
    0x3807, 0xA8,
    0x3808, 0x0A, // DVP width = 2600
    0x3809, 0x28,
    0x380A, 0x07, // DVP height = 1960
    0x380B, 0xA8,

    0x380C, 0x0C, // Total Horizontal Size = 3238
    0x380D, 0xA6,
    0x380E, 0x07, // Total Vertical Size = 1931
    0x380F, 0xB8,

    0x3815, 0x82,
    0x3818, 0xC0, // TIMING CONTROL 18, [6]: mirror, [5]: vertical flip, [0]: vsub2, [1]: vsub4
    0x381A, 0x1C,
    0x381C, 0x04,
    0x381D, 0x04,
    0x381E, 0x07,
    0x381F, 0xC8,
    0x3820, 0x00,
    0x3821, 0x20,
    0x3824, 0x22,
    0x3825, 0x50,
    0x3613, 0x44,
    0x3621, 0x2F, // [7]: 1: H-sub, [4]: 0: if 0x3818[6]=1, 1: if 0x3818[6]=0
    0x3703, 0xE6,
    0x3705, 0xDA,
    0x370A, 0x80,
    0x370C, 0x00,
    0x370D, 0x04,
    0x3713, 0x22,
    0x3714, 0x27,
    0x401C, 0x46,
    0x5002, 0x00, // ISP CONTROL 02, [1]: vap_en
    0x505A, 0x0A,
    0x505B, 0x2E,
    0x5901, 0x00, // VAP CONTROL 01, [3:2]: hsub_coef, [1:0]: vsub_coef
    0x3010, 0x10,
    0x3011, 0x14,
};

MMP_USHORT SNR_OV5650_Reg_1920x1080_Customer[] = 
{
    //1944x1096 30fps ok
    0x3800, 0x02, // HREF Horizontal Start Point High Byte [3:0]
    0x3801, 0x22, // HREF Horizontal Start Point Low Byte
    0x3803, 0x0C, // HREF Vertical Start Point Low Byte ==> 0x10

    0x3804, 0x07, // HREF width = 1944
    0x3805, 0x98,
    0x3806, 0x04, // VREF height = 1096
    0x3807, 0x48,
    0x3808, 0x07, // DVP width = 1944
    0x3809, 0x98,
    0x380A, 0x04, // DVP height = 1096
    0x380B, 0x48,

    0x380C, 0x0C, // Total Horizontal Size = 2877
    0x380D, 0xA6,
    0x380E, 0x04, // Total Vertical Size = 1112
    0x380F, 0x58,

    0x3815, 0x82,
    0x3818, 0xC0, // TIMING CONTROL 18, [6]: mirror, [5]: vertical flip, [0]: vsub2, [1]: vsub4
    0x381A, 0x1C,
    0x381C, 0x31,
    0x381D, 0xBA,
    0x381E, 0x04,
    0x381F, 0x68,
    0x3820, 0x05,
    0x3821, 0x19,
    0x3824, 0x22,
    0x3825, 0xD8,

    //0x5001, 0x00, //sensor AWB disable

    0x3613, 0x44,
    0x3621, 0x2F, // [7]: 1: H-sub, [4]: 0: if 0x3818[6]=1, 1: if 0x3818[6]=0
    0x3703, 0xE6,
    0x3705, 0xDA,
    0x370A, 0x80,
    0x370C, 0x00,
    0x370D, 0x04,
    0x3713, 0x22,
    0x3714, 0x27,
    0x401C, 0x46,
    0x5002, 0x00, // ISP CONTROL 02, [1]: vap_en
    0x505A, 0x0A,
    0x505B, 0x2E,
    0x5901, 0x00, // VAP CONTROL 01, [3:2]: hsub_coef, [1:0]: vsub_coef
    0x3010, 0x10,
    0x3011, 0x14,
};

MMP_USHORT SNR_OV5650_Reg_1280x960_Customer[] = 
{
    //1296x976 30fps ok
    0x3800, 0x02, // HREF Horizontal Start Point High Byte [3:0]
    0x3801, 0x22, // HREF Horizontal Start Point Low Byte
    0x3803, 0x0C, // HREF Vertical Start Point Low Byte ==> 0x10

    0x3804, 0x05, // HREF width = 1296
    0x3805, 0x10,
    0x3806, 0x03, // VREF height = 976
    0x3807, 0xD0,
    0x3808, 0x05, // DVP width = 1296
    0x3809, 0x10,
    0x380A, 0x03, // DVP height = 976
    0x380B, 0xD0,

    0x380C, 0x0C, // Total Horizontal Size = 3225
    0x380D, 0x99,
    0x380E, 0x03, // Total Vertical Size = 992
    0x380F, 0xE0,

    0x3815, 0x81,
    0x3818, 0xC1, // TIMING CONTROL 18, [6]: mirror, [5]: vertical flip, [0]: vsub2, [1]: vsub4
    0x381A, 0x00,
    0x381C, 0x30,
    0x381D, 0x02,
    0x381E, 0x07,
    0x381F, 0xC0,
    0x3820, 0x00,
    0x3821, 0x20,
    0x3824, 0x23,
    0x3825, 0x22,
    0x3613, 0xC4,
    0x3621, 0xAF, // [7]: 1: H-sub, [4]: 0: if 0x3818[6]=1, 1: if 0x3818[6]=0
    0x3703, 0x9A,
    0x3705, 0xDB,
    0x370A, 0x81,
    0x370C, 0x00,
    0x370D, 0x42,
    0x3713, 0x92,
    0x3714, 0x17,
    0x401C, 0x42,
    0x5002, 0x00, // ISP CONTROL 02, [1]: vap_en
    0x505A, 0x0A,
    0x505B, 0x2E,
    0x5901, 0x00, // VAP CONTROL 01, [3:2]: hsub_coef, [1:0]: vsub_coef
    0x3010, 0x10,
    0x3011, 0x14,
};

//extern void MMPF_Do_Mirror_Flip();
void SNR_OV5650_SetSensorResolution_Customer(ISP_SENSOR_RESOL res)
{
#if USB_UVC_1080P_YUY2_EN || USB_UVC_1080P_MJPEG_EN
    PCAM_USB_VIDEO_FORMAT vidFmt = pcam_get_info()->pCamVideoFormat;
#endif
    ISP_UINT32 i;
    // implement your change sensor resolution routine here =======================================

    MMP_ULONG VIFGrab_H_Start, VIFGrab_H_Length, VIFGrab_V_Start, VIFGrab_V_Length;
    MMP_ULONG ulVsync_tmp = 0;  //for MJPEG FHD

    switch (res) {
    case ISP_SENSOR_RESOL_2560x1920:
        
        // set register...........
        for (i = 0; i < VR_ARRSIZE(SNR_OV5650_Reg_2560x1920_Customer)/2; i++) {
            gsSensorFunction->MMPF_Sensor_SetReg(SNR_OV5650_Reg_2560x1920_Customer[i*2], SNR_OV5650_Reg_2560x1920_Customer[i*2+1]);
        }
#if USB_UVC_1080P_YUY2_EN
        if((vidFmt == PCAM_USB_VIDEO_FORMAT_YUV422) || (vidFmt == MMPS_3GPRECD_VIDEO_FORMAT_YUV420)){
            gsSensorFunction->MMPF_Sensor_SetReg(0x3010, 0x70);
            if(glPccamResolution==22)//1600x1200
                gsSensorFunction->MMPF_Sensor_SetReg(0x3010, 0x30);
            if(glPccamResolution==25)//2592x1944
                gsSensorFunction->MMPF_Sensor_SetReg(0x3010, 0xA0);
        }
#endif

        // set target fps and corresponding sensor vsync
        ISP_IF_AE_SetSensorVsync(0x07B8, 30*10);//bossino fix m2ts AV sync @0928 ISP_IF_AE_SetSensorVsync(0x0348, 30*10);

        // set vif grab range which is fetched to ISP (for lens shading and 3A window calculation)	
        VIFGrab_H_Start     = 1;
        VIFGrab_H_Length    = 2592+8;
        VIFGrab_V_Start     = 1;
        VIFGrab_V_Length    = 1944+16;

        ISP_IF_IQ_SetISPInputLength(VIFGrab_H_Length, VIFGrab_V_Length);

        // set scaler downsample rate (for scaler calculation)
        //ISP_IF_SNR_SetDownSample(1, 1);

        // set color id
        #if SENSOR_ROTATE_180
        ISP_IF_IQ_SetColorID(3);
        #else
        ISP_IF_IQ_SetColorID(0);// mirror is 3
        #endif		
        /*
        // set sensor grab range
        ISP_IF_IQ_SetSensorGrab(    0,                  //H_Offest
                                    0,                  //V_Offest
                                    VIFGrab_H_Length,   //H_Length
                                    VIFGrab_V_Length,   //V_Length
                                    1, 1,               //H_Scale_N, H_Scale_M
                                    1, 1);              //V_Scale_N, V_Scale_M
        */
        // calcuate shading ratio
        {
            ISP_UINT32 base_x, base_y;
            ISP_UINT32 x_scale_n = 0x200;
            ISP_UINT32 y_scale_n = 0x200;
            ISP_IF_IQ_GetCaliBase(&base_x, &base_y);

            x_scale_n = VIFGrab_H_Length * 0x200 / base_x;
            y_scale_n = VIFGrab_V_Length * 0x200 / base_y;

            ISP_IF_IQ_SetCaliRatio( x_scale_n,  //x_scale_n, 
                                    0x200,      //x_scale_m, 
                                    0,          //x_offset, 
                                    y_scale_n,  //y_scale_n, 
                                    0x200,      //y_scale_m, 
                                    0);         //y_offset
        }

        ISP_IF_AE_SetBaseShutter( BASE_SHUTTER_50HZ_2560x1920, BASE_SHUTTER_60HZ_2560x1920);

        break;

    case ISP_SENSOR_RESOL_1920x1080:
        // set register...........
        for (i = 0; i < VR_ARRSIZE(SNR_OV5650_Reg_1920x1080_Customer)/2; i++) {
            gsSensorFunction->MMPF_Sensor_SetReg(SNR_OV5650_Reg_1920x1080_Customer[i*2], SNR_OV5650_Reg_1920x1080_Customer[i*2+1]);
        }
#if USB_UVC_1080P_YUY2_EN
        if((vidFmt == PCAM_USB_VIDEO_FORMAT_YUV422) || (vidFmt == MMPS_3GPRECD_VIDEO_FORMAT_YUV420)){
            gsSensorFunction->MMPF_Sensor_SetReg(0x3010, 0x70);
            ulVsync_tmp = 0;
        }
#endif
#if USB_UVC_1080P_MJPEG_EN
        if(vidFmt == PCAM_USB_VIDEO_FORMAT_MJPEG){
            gsSensorFunction->MMPF_Sensor_SetReg(0x3010, 0x10);
            gsSensorFunction->MMPF_Sensor_SetReg(0x3012, 0x01);
            ulVsync_tmp = 216; //Fix MJPEG FHD 30 fps
        }
#endif

        // set target fps and corresponding sensor vsync
        //ISP_IF_AE_SetSensorVsync(0x046c, 30*10);//bossino fix m2ts AV sync @0928 ISP_IF_AE_SetSensorVsync(0x0348, 30*10);
        ISP_IF_AE_SetSensorVsync(0x0458 + ulVsync_tmp, 30*10);//bossino fix m2ts AV sync @0928 ISP_IF_AE_SetSensorVsync(0x0348, 30*10);

        // set vif grab range which is fetched to ISP (for lens shading and 3A window calculation)
        VIFGrab_H_Start     = 1;
        VIFGrab_H_Length    = 1920+8;
        VIFGrab_V_Start     = 1;
        VIFGrab_V_Length    = 1080+16;

        ISP_IF_IQ_SetISPInputLength(VIFGrab_H_Length, VIFGrab_V_Length);

        // set scaler downsample rate (for scaler calculation)
        //ISP_IF_SNR_SetDownSample(1, 1);

        // set color id
        #if SENSOR_ROTATE_180
        ISP_IF_IQ_SetColorID(3);
        #else
        ISP_IF_IQ_SetColorID(0);// mirror is 3
        #endif		

        /*
        // set sensor grab range
        ISP_IF_IQ_SetSensorGrab(    0,                  //H_Offest
                                    0,                  //V_Offest
                                    VIFGrab_H_Length,   //H_Length
                                    VIFGrab_V_Length,   //V_Length
                                    1, 1,               //H_Scale_N, H_Scale_M
                                    1, 1);              //V_Scale_N, V_Scale_M
        */
        		// calcuate shading ratio
        {
            ISP_UINT32 base_x, base_y;
            ISP_UINT32 x_scale_n = 0x200;
            ISP_UINT32 y_scale_n = 0x200;
            ISP_IF_IQ_GetCaliBase(&base_x, &base_y);

            x_scale_n = VIFGrab_H_Length * 0x200 / base_x;
            y_scale_n = VIFGrab_V_Length * 0x200 / base_y;

            ISP_IF_IQ_SetCaliRatio( x_scale_n,  //x_scale_n, 
                                    0x200,      //x_scale_m, 
                                    0,          //x_offset, 
                                    y_scale_n,  //y_scale_n, 
                                    0x200,      //y_scale_m, 
                                    0);         //y_offset
        }

        ISP_IF_AE_SetBaseShutter( BASE_SHUTTER_50HZ_1920x1080, BASE_SHUTTER_60HZ_1920x1080);

        break;

    case ISP_SENSOR_RESOL_1280x960:
    case ISP_SENSOR_RESOL_1280x720:

        // set register...........
        for (i = 0; i < VR_ARRSIZE(SNR_OV5650_Reg_1280x960_Customer)/2; i++) {
            gsSensorFunction->MMPF_Sensor_SetReg(SNR_OV5650_Reg_1280x960_Customer[i*2], SNR_OV5650_Reg_1280x960_Customer[i*2+1]);
        }

        // set target fps and corresponding sensor vsync
        ISP_IF_AE_SetSensorVsync(0x03E0, 30*10);

        // set vif grab range which is fetched to ISP (for lens shading and 3A window calculation)
        VIFGrab_H_Start     = 1;
        VIFGrab_H_Length    = 1296;
        VIFGrab_V_Start     = 1;
        VIFGrab_V_Length    = 972;

        ISP_IF_IQ_SetISPInputLength(VIFGrab_H_Length, VIFGrab_V_Length);

        // set scaler downsample rate (for scaler calculation)
        //ISP_IF_SNR_SetDownSample(1, 1);

        // set color id
        #if SENSOR_ROTATE_180
        ISP_IF_IQ_SetColorID(3);
        #else
        ISP_IF_IQ_SetColorID(0);// mirror is 3
        #endif		
        /*
        // set sensor grab range
        ISP_IF_IQ_SetSensorGrab(    0,                  //H_Offest
                                    0,                  //V_Offest
                                    VIFGrab_H_Length,   //H_Length
                                    VIFGrab_V_Length,   //V_Length
                                    1, 1,               //H_Scale_N, H_Scale_M
                                    1, 1);              //V_Scale_N, V_Scale_M
        */
        // calcuate shading ratio
        {
            ISP_UINT32 base_x, base_y;
            ISP_UINT32 x_scale_n = 0x200;
            ISP_UINT32 y_scale_n = 0x200;
            ISP_IF_IQ_GetCaliBase(&base_x, &base_y);

            x_scale_n = VIFGrab_H_Length * 0x200 / base_x;
            y_scale_n = VIFGrab_V_Length * 0x200 / base_y;

            ISP_IF_IQ_SetCaliRatio( x_scale_n,  //x_scale_n, 
                                    0x200,      //x_scale_m, 
                                    0,          //x_offset, 
                                    y_scale_n,  //y_scale_n, 
                                    0x200,      //y_scale_m, 
                                    0);         //y_offset
        }

        ISP_IF_AE_SetBaseShutter( BASE_SHUTTER_50HZ_1280x960, BASE_SHUTTER_60HZ_1280x960);
        break;
    }

    // end of your implementation =================================================================

    // set VIF OPR (Actual sensor output size)
    {
        AITPS_VIF  pVIF = AITC_BASE_VIF;
        volatile MMP_BYTE* pISP = (volatile MMP_BYTE*)AITC_BASE_ISP;
        //MMP_UBYTE   vid = 0;

        pVIF->VIF_GRAB[vif_id].PIXL_ST = VIFGrab_H_Start;
        pVIF->VIF_GRAB[vif_id].PIXL_ED = VIFGrab_H_Start + VIFGrab_H_Length -1;
        pVIF->VIF_GRAB[vif_id].LINE_ST = VIFGrab_V_Start ;
        pVIF->VIF_GRAB[vif_id].LINE_ED = VIFGrab_V_Start + VIFGrab_V_Length -1;
        pVIF->VIF_INT_LINE_NUM_0[vif_id] = VIFGrab_V_Length - 61;//121;

#if 1	
        dbg_printf(3,"ViF(xs,xe,ys,ye)=(%d,%d,%d,%d)\r\n",pVIF->VIF_GRAB[vif_id].PIXL_ST,pVIF->VIF_GRAB[vif_id].PIXL_ED,pVIF->VIF_GRAB[vif_id].LINE_ST,pVIF->VIF_GRAB[vif_id].LINE_ED);
        dbg_printf(3, "[0] VIFGrab_H_Length = %d, VIFGrab_V_Length = %d\r\n", VIFGrab_H_Length, VIFGrab_V_Length); 
#endif

        if (m_gsISPCoreID == 868) {
            // ISP pipeline selection
            //APICAL_XBYTE[APICAL_CTL_REG1] &= ~(0x80); // clear auto size
            pISP[0x0B] = 0x30;
            pISP[0x09] = 0x0C;

            // IP ColorID setting
            //pISP[0x09] &= ~(0xF0);
            //pISP[0x09] |= (ISP_IF_SNR_GetColorID() == 0 ? 3 : (ISP_IF_SNR_GetColorID() == 1 ? 2 : (ISP_IF_SNR_GetColorID() == 2 ? 1: 0))) * (0x50);
        }
    }
    //================
    // set iq (NR, Edge, CCM, Gamma, etc.) and functions (saturation, contrast, sharpness, hue, etc.)
    ISP_IF_IQ_SetAll();

    // SetBeforePreview and SetBeforeCapture should be called after getting new configurations of the current resolution
    if (gSystemMode  == VENUS_SYSTEM_MODE_SNAPSHOT) {
        // update AE before capture
        ISP_IF_AE_UpdateBeforeCapture();

        // update AWB before capture
        ISP_IF_AWB_UpdateBeforeCapture();

        // special settings for capture could be set here =============================================
        {
        		}
        // end of special settings ====================================================================
    } else if (gSystemMode == VENUS_SYSTEM_MODE_PREVIEW) {
        // update AE before preview
        ISP_IF_AE_UpdateBeforePreview();

        // update AWB before preview
        ISP_IF_AWB_UpdateBeforePreview();
    }

    // set direction (color ID, orientation, etc.)
    ISP_IF_IQ_SetDirection(ISP_IQ_DIRECTION_ORIGINAL);

    // set exposure parameters
    ISP_IF_IQ_SetAEGain(ISP_IF_AE_GetDGain());
    gsSensorFunction->MMPF_Sensor_SetSensorGain(ISP_IF_AE_GetAGain());
    gsSensorFunction->MMPF_Sensor_SetSensorShutter(ISP_IF_AE_GetShutter(), ISP_IF_AE_GetVsync());

    // update ISP window
    ISP_IF_IQ_UpdateInputSize();

    // check if modules are bypassed
    ISP_IF_IQ_CheckBypass();

    // force update iq to hardware
    ISP_IF_IQ_UpdateOprtoHW(ISP_IQ_SWITCH_ALL, 1);

}

#if SUPPORT_AUTO_FOCUS
void MOTOR_OV5650_VCM_InitMotor_Customer(void)
{
#if (CUSTOMER == LON) || ((CUSTOMER == LGT)&&(MODEL == 3)) || (CUSTOMER == ASU)
    //	MMP_USHORT ret;
    // initialize I2C ID and type
    //VR_MOTOR_ConfigI2C(	0x0C, ISP_I2C_TYPE_1A1D);

    ISP_IF_AF_SetSearchRange(0x30 * 4, 0x98 * 4, 0x90 * 4, 0xC0 * 4, 10);

    //MMPF_MOTOR_WriteI2C(0xEC, 0xA3);  // Protection off
    //MMPF_MOTOR_WriteI2C(0xA1, 0x05);  // DLC/MCLK setting
    //MMPF_MOTOR_WriteI2C(0xDC, 0x51);  // Protection on
#endif

#if ((CUSTOMER == CCN)&&(MODEL == 2))
    {
    MMP_USHORT read_value;

    //VR_MOTOR_ConfigI2C(	0x0E, ISP_I2C_TYPE_1A1D);  // for DRV201

    ISP_IF_AF_SetSearchRange(46 * 4, 96 * 4, 90 * 4, 160 * 4, 10);

    read_value = MMPF_MOTOR_ReadI2C(0x00);  // Read IC_ID, should be 0x01.
    RTNA_DBG_Str(3, " Read DRV201 Reg[0x00] = ");       
    RTNA_DBG_Short(3, read_value);  
    RTNA_DBG_Str(3, "\r\n");  
    MMPF_MOTOR_ReadI2C(0x01, &read_value);  // Read IC_VER, should be 0x01 or 0x02 or 0x03
    RTNA_DBG_Str(3, " Read DRV201 Reg[0x01] = ");       
    RTNA_DBG_Short(3, read_value);  
    RTNA_DBG_Str(3, "\r\n");  

    MMPF_MOTOR_WriteI2C(0x02, 0x02);  // [0]: 1: device goes to standby, 0: inactive, [1]: enable ringing compensation
    MMPF_MOTOR_WriteI2C(0x07, 0x60);  // VCM Resonance Frequency Register, VCM_FREQ = (383 - (19200/Fres) - 1), Fres = 67.1Hz 

    }
#endif
	
    dbg_printf(3, "###MOTOR_OV5650_VCM_InitMotor_Customer###\r\n");
}


void MOTOR_OV5650_VCM_EnterStandby_Customer(MMP_USHORT af_pos)
{
    //VR_Motor_SmoothMove(af_pos);

#if ((CUSTOMER == CCN)&&(MODEL == 2))  // for DRV201
    MMPF_MOTOR_WriteI2C(0x02, 0x03);  // bit 0: 1: device goes to standby, 0: inactive
#endif
}

void MOTOR_OV5650_VCM_SetAFPosition_Customer(MMP_USHORT af_pos)
{
#if (CUSTOMER == LON) || ((CUSTOMER == LGT)&&(MODEL == 3)) || (CUSTOMER == ASU)
#if 1//AF_10BIT_INTERFACE
    // =================================
    // AD5820(MD118) data format
    // SDA = --------+--------+--------
    //		 ---11--W|S-987654|3210----
    //          ID      addr     data
    //
    // addr = (gsAFPos >> 4) & 0x3f
    // data = (gsAFPos << 4) & 0xf0
    // =================================
    MMPF_MOTOR_WriteI2C((af_pos >> 4) & 0x3f, (af_pos << 4) & 0xf0);
#else
    // =================================
    // AD5820(MD118) data format
    // SDA = --------+--------+--------
    //		 ---11--W|S-765432|10------
    //          ID      addr     data
    //
    // addr = (gsAFPos >> 2) & 0x3f
    // data = (gsAFPos << 6) & 0xc0
    // =================================
    MMPF_MOTOR_WriteI2C((af_pos >> 2) & 0x3f, (af_pos << 6) & 0xc0);
#endif
#endif

#if ((CUSTOMER == CCN)&&(MODEL == 2))  // for DRV201
    // =================================
    // DRV201 data format
    // reg 0x03 = (gsAFPos >> 6) & 0x03
    // reg 0x04 = (gsAFPos << 2) & 0xff
    // =================================
#if 1//AF_10BIT_INTERFACE
    MMPF_MOTOR_WriteI2C(0x03, (MMP_UBYTE)((af_pos >> 8) & 0x03));
    MMPF_MOTOR_WriteI2C(0x04, (MMP_UBYTE)((af_pos << 0) & 0xff));
#else
    MMPF_MOTOR_WriteI2C(0x03, (MMP_UBYTE)((af_pos >> 6) & 0x03));
    MMPF_MOTOR_WriteI2C(0x04, (MMP_UBYTE)((af_pos << 2) & 0xff));
#endif

#endif

    //dbg_printf(3, "af_pos = %d\r\n", af_pos);
}

MMP_UBYTE MOTOR_OV5650_VCM_CheckAFreachPosition_Customer(MMP_USHORT af_pos)
{
    
    return 1;
    
}
#endif

#if (SENSOR_ID_OV5650== 0)
MMPF_SENSOR_FUNCTION  *SensorFunction_Module0 = &SensorFunction_OV5650;
#endif

#if (SENSOR_ID_OV5650== 1)
MMPF_SENSOR_FUNCTION  *SensorFunction_Module1 = &SensorFunction_OV5650;
#endif

#endif  //BIND_SENSOR_OV5650
#endif
